#include "World.h"

#include "Remote.h"
#include "KalmanTank.h"
#include "KalmanFilter.h"
#include "KalmanObstacle.h"

World* World::_instance = 0;
float THRESHOLD = .9;

World::World() {}


World::~World() {
}

void World::init(){
	BZRC* com = Remote::instance();

	_constants.clear();
	_flags.clear();
	_tanks.clear();
	_obstacles.clear();
	_other_tanks.clear();
	_shots.clear();
	_teams.clear();

	std::vector<Command> init;
	init.push_back(Command(-1, -1, -1, false, false, CONSTANTS | OBSTACLES | TEAMS | MY_TANKS | OTHER_TANKS | FLAGS | SHOTS));
	com->do_commands(init);

	_occgrids.resize(_tanks.size());

	for(unsigned int i = 0; i < _constants.size(); i ++){
		if(_constants[i].name.compare("worldsize") == 0){
			size = std::atoi(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("truepositive") == 0){
			truePositive = std::atof(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("truenegative") == 0){
			trueNegative = std::atof(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("shotspeed") == 0){
			bulletSpeed = std::atoi(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("shotrange") == 0){
			shotRange = std::atoi(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("team") == 0){
			team = _constants[i].value.c_str();
		}
	}

	for(unsigned int i = 0; i < _other_tanks.size(); i ++){
		_enemies.push_back(KalmanTank(&_other_tanks[i]));
	}

	for (unsigned int i = 0; i < _obstacles.size(); i++) {
		_kobstacles.push_back(KalmanObstacle(&_obstacles[i]));
	}

	graph = new Graph(4);
	graph->buildEdges();
}

World* World::instance() {
	if (!_instance)
		_instance = new World();

	return _instance;
}

bool World::intersectsAnObstacle(Vector2 & start, Vector2 & end) {

	for (unsigned int i = 0; i < kobstacles().size(); i++) {
		KalmanObstacle* ob = &kobstacles().at(i);
		for (unsigned int j = 1; j < ob->corners().size(); j++) {
			Vector2 ob_start = ob->corners().at(j - 1).GetCurrentEstimate();
			Vector2 ob_end = ob->corners().at(j).GetCurrentEstimate();
			if (linesIntersect(start, end, ob_start, ob_end)) {
				return true;
			}
		}
	}
	return false;
}

bool World::linesIntersect(Vector2 & start, Vector2 & end, Vector2 & ob_start, Vector2 & ob_end) {
	Vector2 p1 = start.x < end.x ? start : end;
	Vector2 p2 = start.x >= end.x ? start : end;
	Vector2 p3 = start.y < end.y ? start : end;
	Vector2 p4 = start.y >= end.y ? start : end;

	Vector2 p5 = ob_start.x < ob_end.x ? ob_start : ob_end;
	Vector2 p6 = ob_start.x >= ob_end.x ? ob_start : ob_end;
	Vector2 p7 = ob_start.y < ob_end.y ? ob_start : ob_end;
	Vector2 p8 = ob_start.y >= ob_end.y ? ob_start : ob_end;
	
	
	int buf = 5;
	p5.x -= buf;
	p6.x += buf;
	p7.y -= buf;
	p8.y += buf;
	/**/

	float m1 = (p1.y - p2.y) / (p1.x - p2.x);
	float b1 = -m1 * p1.x + p1.y;
	float m2 = (p5.y - p6.y) / (p5.x - p6.x);
	float b2 = -m2 * p5.x + p5.y;

	float x = (p1.x - p2.x) == 0 ?
		p1.x :
		(b2 - b1) / (m1 - m2);
	float y = (p1.x - p2.x) == 0 ?
		((p5.x - p6.x) == 0 ?
			p5.x :
			m2 * x + b2) :
		m1 * x + b1;

	if (p1.x <= x && x <= p2.x &&
		p3.y <= y && y <= p4.y &&
		p5.x <= x && x <= p6.x &&
		p7.y <= y && y <= p8.y) {
		return true;
	}

	return false;
}

Graph* World::getGraph() {
	return graph;
}

Vector2 World::getFlagPos(string flag_color) {
	for (unsigned int i = 0; i < flags().size(); i++) {
		flag_t* flag = &flags().at(i);
		if (flag->color == flag_color) {
			return Vector2(flag->pos[0], flag->pos[1]);
		}
	}

	return Vector2(-1,-1);
}

Vector2 World::getBasePos(string base_color) {
	for (unsigned int i = 0; i < teams().size(); i++) {
		team_t* team = &teams().at(i);

		if (team->color.compare(base_color) == 0) {
			Vector2 base_pos;

			for(int i = 0; i < 4; i ++){
				base_pos.x += team->base_corner[i][0];
				base_pos.y += team->base_corner[i][1];
			}
			base_pos.x /= 4.0;
			base_pos.y /= 4.0;

			return base_pos;
		}
	}

	return Vector2(-1,-1);
}

Command World::update() {
	BZRC* com = Remote::instance();

	return Command(-1, -1, -1, false, false, OBSTACLES | MY_TANKS | OTHER_TANKS | FLAGS | SHOTS);
}

vector <constant_t> & World::constants() {
	return _constants;
}

vector <flag_t> & World::flags() {
	return _flags;
}

vector <tank_t> & World::tanks() {
	return _tanks;
}

vector <obstacle_t> & World::obstacles() {
	return _obstacles;
}

vector <otank_t> & World::other_tanks() {
	return _other_tanks;
}

vector <shot_t> & World::shots() {
	return _shots;
}

vector <team_t> & World::teams() {
	return _teams;
}
vector <occgrid_t> & World::occgrids(){
	return _occgrids;
}

vector <KalmanTank> & World::enemies(){
	return _enemies;
}

vector <KalmanObstacle> & World::kobstacles() {
	return _kobstacles;
}

vector <constant_t> * World::constantsP() {
	return &_constants;
}

vector <flag_t> * World::flagsP() {
	return &_flags;
}

vector <tank_t> * World::tanksP() {
	return &_tanks;
}

vector <obstacle_t> * World::obstaclesP() {
	return &_obstacles;
}

vector <otank_t> * World::other_tanksP() {
	return &_other_tanks;
}

vector <shot_t> * World::shotsP() {
	return &_shots;
}

vector <team_t> * World::teamsP() {
	return &_teams;
}

vector <occgrid_t> * World::occgridsP(){
	return &_occgrids;
}

vector <KalmanTank> * World::enemiesP(){
	return &_enemies;
}

vector <KalmanObstacle> * World::kobstaclesP() {
	return &_kobstacles;
}

int World::getSize(){
	return size;
}

float World::getTruePositive(){
	return truePositive;
}

float World::getTrueNegative(){
	return trueNegative;
}

int World::getBulletSpeed(){
	return bulletSpeed;
}

int World::getShotRange(){
	return shotRange;
}

string World::getTeam(){
	return team;
}

void World::updateKalmanFilters() {
	for (int i = 0; i < _enemies.size(); i++) {
		updateKalmanFilter(_enemies[i]);
	}

	for (int i = 0; i < _kobstacles.size(); i++) {
		for (int j = 0; j < _kobstacles[i].corners().size(); j++) {
			updateKalmanFilter(_kobstacles[i].corners().at(j));
		}
	}
}

bool World::TeamMemberHasFlag(){
	for(int i = 0; i < _tanks.size(); i ++){
		if(_tanks[i].flag.compare("-") != 0){
			_flagBearer = &_tanks[i];
			return true;
		}
	}
	return false;
}

void World::flagBearerDir(Vector2 newDir){
	_flagBearerDir = newDir;
}

Vector2 World::flagBearerDir(){
	return _flagBearerDir;
}

void World::flagBearer(tank_t* tank){
	_flagBearer = tank;
}

tank_t* World::flagBearer(){
	return _flagBearer;
}

bool World::blocked(Vector2 & start, Vector2 & end){
	for(int i = 0; i < _kobstacles.size(); i ++){
		for(int j = 0; j < _kobstacles[i].corners().size(); j ++){
			int current = j;
			int next = j + 1;
			if(current+1 == _kobstacles[i].corners().size())
				next = 0;
			if(linesIntersect(start, end, _kobstacles[i].corners()[current].GetCurrentEstimate(), _kobstacles[i].corners()[next].GetCurrentEstimate())){
				return true;
			}
		}
	}
	return false;
}

void World::updateKalmanFilter(KalmanFilter & filter) {
	if (filter.ObjectIsOnBoard()) {
		filter.UpdateMatricies();
	}
	else {
		filter.ResetEx();
	}
}